/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "ownutils.h"

static float ccos[361];
static float csin[361];

void cache_cos_sin(){
		for(int i=0;i<360;i++){
				ccos[i]=cos(i*M_PI/180.0);
				csin[i]=sin(i*M_PI/180.0);
		}
}

float cosi(int angle){
		if(angle<361){
				return ccos[angle];
		}
		return 0;
}
float sini(int angle){
		if(angle<361){
				return csin[angle];
		}
		return 0;
}


int main(void){
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss1,&us1);
		cache_cos_sin();
		float x; float y;
		float x1,y1;
		float z=0;
		long long int iso = (1000*1000*20*180);
		
		fprintf(stderr,"iso luku on %lld\n",iso);
		
		for(int i=0;i<180;i++){
				for(int angles=0;angles<180;angles++){
						x = 10.0 * cos(M_PI*angles/180.0);	
						y = 10.0 * sin(M_PI*angles/180.0);
				}
				for(int ix = 0; ix< 1000;ix++ ){
						for(int j=0;j<1000;j++){
								x1 = x + (float)(ix)*0.1;	
								y1 = y + ((float) j)*0.1;	
								z += 0.00001*x1*y1;
								if(z>1100) z = 0;
						}
				}
		}
				
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"1: (%.6f=%.6f) %f \n",fms,fms/1000.0,z);
		
		return 0;
}